#include "main_motor.h"
#include "timer_utils.h"
#include "actuator_control.h"
#include <string.h>

// 模块内部状态
static struct {
    MotorConfig config;
    MotorState state;
    MotorError error;
    uint32_t start_time;
    uint32_t retry_count;
    uint32_t protection_start;
    bool current_normal;
} s_motor;

void motor_init(const MotorConfig* config) {
    memset(&s_motor, 0, sizeof(s_motor));
    
    if (config != NULL) {
        s_motor.config = *config;
    } else {
        // 默认配置
        s_motor.config = (MotorConfig){
            .min_current = 200,
            .max_current = 800,
            .start_timeout = 5000,
            .retry_count = 3,
            .protection_time = 30000
        };
    }
    
    s_motor.state = MOTOR_STATE_STOPPED;
    s_motor.error = MOTOR_ERROR_NONE;
}

void motor_start(void) {
    if (s_motor.state == MOTOR_STATE_PROTECTION) {
        // 保护状态下无法启动
        return;
    }
    
    if (s_motor.state != MOTOR_STATE_STOPPED) {
        return;
    }
    
    // 启动电机
    actuator_set_main_motor(true);
    s_motor.state = MOTOR_STATE_STARTING;
    s_motor.start_time = get_system_time_ms();
    s_motor.error = MOTOR_ERROR_NONE;
}

void motor_stop(void) {
    if (s_motor.state == MOTOR_STATE_STOPPED) {
        return;
    }
    
    // 停止电机
    actuator_set_main_motor(false);
    s_motor.state = MOTOR_STATE_STOPPED;
    s_motor.retry_count = 0;
    s_motor.error = MOTOR_ERROR_NONE;
}

void motor_process(uint32_t current) {
    uint32_t now = get_system_time_ms();
    
    // 检查电流是否正常
    s_motor.current_normal = (current >= s_motor.config.min_current && 
                             current <= s_motor.config.max_current);
    
    switch (s_motor.state) {
        case MOTOR_STATE_STOPPED:
            // 停止状态不处理
            break;
            
        case MOTOR_STATE_STARTING:
            // 检查启动超时
            if (now - s_motor.start_time > s_motor.config.start_timeout) {
                if (!s_motor.current_normal) {
                    // 启动失败
                    s_motor.error = current < s_motor.config.min_current ? 
                                   MOTOR_ERROR_UNDER_CURRENT : MOTOR_ERROR_OVER_CURRENT;
                    
                    s_motor.retry_count++;
                    actuator_set_main_motor(false);
                    
                    if (s_motor.retry_count >= s_motor.config.retry_count) {
                        // 达到最大重试次数，进入保护
                        s_motor.state = MOTOR_STATE_PROTECTION;
                        s_motor.error = MOTOR_ERROR_START_FAILED;
                        s_motor.protection_start = now;
                    } else {
                        // 重试启动
                        s_motor.state = MOTOR_STATE_STOPPED;
                    }
                } else {
                    // 启动成功
                    s_motor.state = MOTOR_STATE_RUNNING;
                }
            }
            break;
            
        case MOTOR_STATE_RUNNING:
            // 运行中检查电流
            if (!s_motor.current_normal) {
                s_motor.error = current < s_motor.config.min_current ? 
                               MOTOR_ERROR_UNDER_CURRENT : MOTOR_ERROR_OVER_CURRENT;
                actuator_set_main_motor(false);
                s_motor.state = MOTOR_STATE_PROTECTION;
                s_motor.protection_start = now;
            }
            break;
            
        case MOTOR_STATE_PROTECTION:
            // 保护时间结束后恢复
            if (now - s_motor.protection_start > s_motor.config.protection_time) {
                s_motor.state = MOTOR_STATE_STOPPED;
                s_motor.error = MOTOR_ERROR_NONE;
                s_motor.retry_count = 0;
            }
            break;
    }
}

MotorState motor_get_state(void) {
    return s_motor.state;
}

MotorError motor_get_error(void) {
    return s_motor.error;
}

void motor_reset_protection(void) {
    s_motor.state = MOTOR_STATE_STOPPED;
    s_motor.error = MOTOR_ERROR_NONE;
    s_motor.retry_count = 0;
}

bool motor_is_normal(void) {
    return (s_motor.state == MOTOR_STATE_RUNNING && s_motor.error == MOTOR_ERROR_NONE);
}
